// ********************* OS.c ***********************

#include "inc/hw_ints.h"
#include "inc/hw_memmap.h"
//#include "inc/hw_timer.h"
#include "inc/hw_types.h"
#include "inc/hw_gpio.h"
#include "inc/lm3s8962.h" 
#include "driverlib/debug.h"
#include "driverlib/interrupt.h"
#include "driverlib/timer.h"
#include "driverlib/systick.h"
#include "driverlib/sysctl.h"
#include "driverlib/gpio.h"
#include "serial.h"
#include "ports.h"
#include "RTOS.h"
#include "ADC.h"
#include "rit128x96x4.h"
#include "OS.h"
#include "Interpreter.h"
#include <stdio.h>
//#include "valvanofifocreater.h"

#define TIMERPRESCALE 4
#define PRIORITY      1

extern void OS_CPU_RunFirstContext (void);
extern void OS_CPU_ContextSwitch (void);

struct os_TCB{
	unsigned long *TopStackPt;
	unsigned long *BottomStackPt;
	unsigned long *StackPt;
	unsigned long StackSize;
	unsigned long TaskID;
	struct os_TCB *NextPt;
	struct os_TCB *PrevPt;
	unsigned long TaskPriority;
	unsigned long TimesRun;
	unsigned long SleepCnt;
	Sema4Type *BlockPt;
	unsigned long Stack[STACK_SIZE];
};
typedef struct os_TCB TCB;


// **************** Globals ********************
TCB ThreadBlocks[NUMBER_OF_THREADS];
void (*functionPt)(void)  = 0;
void (*function2Pt)(void) = 0;
void (*selectFunctionPt)(void) = 0;
void (*downFunctionPt)(void)   = 0;
int NextID;
int NumPeriodicThreads;
TCB *CurrentThreadPt;
unsigned long SystemTime = 0;
unsigned long volatile Fifo_Put_Pt; 
unsigned long volatile Fifo_Get_Pt; 
static unsigned long Fifo[FIFO_SIZE]; 
unsigned long MailBox;
unsigned long TimeSlice;
unsigned long PeriodicTime;
TCB *FirstThreadPt;
Sema4Type Sema4_MailBox_Valid;
Sema4Type Sema4_MailBox_Free;
#define JITTERSIZE 100
long MaxJitter1;             // largest time jitter between interrupts in usec
long MinJitter1;             // smallest time jitter between interrupts in usec
unsigned long const JitterSize=JITTERSIZE;
unsigned long JitterHistogram1[JITTERSIZE]={0,};
int Periodic1Index=0;
long Periodic1Count=0;
unsigned long Period1;
long MaxJitter2;             // largest time jitter between interrupts in usec
long MinJitter2;             // smallest time jitter between interrupts in usec
unsigned long JitterHistogram2[JITTERSIZE]={0,};
int Periodic2Index=0;
long Periodic2Count=0;
unsigned long Period2;
unsigned long TaskRecordID[RECORD_SIZE]={0xFFFF0000};
unsigned long TaskRecordT[RECORD_SIZE]={0};
unsigned long RecordIndex=0;
#include "osdebug.h"

void OS_DisableIntCountStart(void);
void OS_DisableIntCountEnd(void);

//********* TaskRecord *********************
// Records current thread's ID and Time
//
void TaskRecord(unsigned long id, unsigned long time){
  if(RecordIndex < RECORD_SIZE){
    TaskRecordID[RecordIndex] = id;
    TaskRecordT[RecordIndex] = time;
    RecordIndex++;
  }
}
//
// ******** Systick Hnadler *********
//
void SysTick_Handler ( void) {
  unsigned long i;

  SystemTime += 1; // Units of TIME_SLICE
  for(i = 0; i < NUMBER_OF_THREADS; i++) {
        // Check if thread is sleeping
    if (ThreadBlocks[i].SleepCnt != 0) {
	      // Decrement sleep count by 2 ms
	  ThreadBlocks[i].SleepCnt -= 1; // Units of TIME_SLICE
	}
  }
  OS_CPU_ContextSwitch();
}

// ******** Timer0IntHandler *******
void Timer0IntHandler(void) {
	unsigned long ulStatus;
	long jitter;
	unsigned static long LastTime;  // time at previous interrupt
    unsigned long thisTime;         // time at current interrupt

	// Get interrupt staus bit
	ulStatus = TimerIntStatus( TIMER0_BASE, true );

	// Acknowledge interrupt
	TimerIntClear( TIMER0_BASE, ulStatus );
	
	thisTime = OS_Time();

	if(Periodic1Count>1){    // ignore timing of first interrupt
      jitter = OS_TimeDifference(thisTime,LastTime)/50-Period1/50;  // in usec
	  if(jitter >100){
	    //GPIO_PORTB_DATA_R |= 0x04;
		jitter = jitter;
	  }
	  if(jitter > MaxJitter1){
        MaxJitter1 = jitter;
      }
      if(jitter < MinJitter1){
        MinJitter1 = jitter;
      }        // jitter should be 0
      Periodic1Index = jitter+JITTERSIZE/2;   // us units
      if(Periodic1Index<0)Periodic1Index = 0;
      if(Periodic1Index>=JitterSize) Periodic1Index = JITTERSIZE-1;
      JitterHistogram1[Periodic1Index]++; 
    }

	LastTime = thisTime;

	// Call Function
	functionPt();
	TaskRecord(25000,SystemTime); // Record periodic task time
	Periodic1Count++;
}

// ******** Timer2IntHandler *******
//
void Timer2IntHandler(void) {
	unsigned long ulStatus;
	long jitter;
	unsigned static long LastTime;  // time at previous ADC sample
    unsigned long thisTime;         // time at current ADC sample

	// Get interrupt staus bit
	ulStatus = TimerIntStatus( TIMER2_BASE, true );
	
	// Acknowledge interrupt
	TimerIntClear( TIMER2_BASE, ulStatus );
	
    thisTime = OS_Time();

	if(Periodic2Count>1){    // ignore timing of first interrupt
      jitter = OS_TimeDifference(thisTime,LastTime)/50-Period2/50;  // in usec
	  //if(jitter > -12)
	    //GPIO_PORTB_DATA_R |= 0x01;
	  if(jitter > MaxJitter2){
        MaxJitter2 = jitter;
      }
      if(jitter < MinJitter2){
        MinJitter2 = jitter;
      }        // jitter should be 0
      Periodic2Index = jitter+JITTERSIZE/2;   // us units
      if(Periodic2Index<0)Periodic2Index = 0;
      if(Periodic2Index>=JitterSize) Periodic2Index = JITTERSIZE-1;
      JitterHistogram2[Periodic2Index]++; 
    }

    LastTime = thisTime;

	// Call Function
	function2Pt();
	TaskRecord(50000,SystemTime); // Record periodic task time
	Periodic2Count++;
}

void SelectIntHandler(void){
  GPIOPinIntClear(GPIO_PORTF_BASE, GPIO_PIN_1);
  selectFunctionPt();
}

void DownIntHandler(void){
  GPIOPinIntClear(GPIO_PORTE_BASE, GPIO_PIN_1);

  downFunctionPt();
}

// ************************
//     Public Functions
// ************************

// ******** OS_Init ************
// initialize operating system, disable interrupts until OS_Launch
// initialize OS controlled I/O: serial, ADC, systick, select switch and timer2 
// input:  none
// output: none
void OS_Init(void){
  unsigned long i,j;

//*SYSTEM CLOCK
  SystemTime = 0;
  
//*THREADS
  FirstThreadPt = NULL;
  CurrentThreadPt = 0;
  NextID = 1;
  for( i = 0; i < NUMBER_OF_THREADS; i++ ) {
    for( j = 0; j < STACK_SIZE; j++) {
	  ThreadBlocks[i].Stack[j] = (j << 16) | 0x0000FFAA; 
	}
	ThreadBlocks[i].TopStackPt    = 0;
	ThreadBlocks[i].BottomStackPt = &(ThreadBlocks[i].Stack[STACK_SIZE - 1]);
	ThreadBlocks[i].StackPt       = &(ThreadBlocks[i].Stack[STACK_SIZE - 1]);
	ThreadBlocks[i].StackSize     = (unsigned long)STACK_SIZE;
	ThreadBlocks[i].TaskID        = 0;
	ThreadBlocks[i].NextPt        = 0;
	ThreadBlocks[i].PrevPt        = 0;
	ThreadBlocks[i].TaskPriority  = 0;
	ThreadBlocks[i].SleepCnt      = 0;
	ThreadBlocks[i].BlockPt       = 0;
  }
  
      // Disable system interrupts
  IntMasterDisable();
  
//*SYSTEM CLOCK
      // Run At 50MHz
      // Set the clocking to run directly from the crystal.
      // Externl Crystal 8MHz and Use PLL and SYSDIV_4
  SysCtlClockSet(SYSCTL_SYSDIV_4 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN |
                   SYSCTL_XTAL_8MHZ);

//*TIMER 0
      // Enable General Purpose Timer 0
  SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER0);
      // Disable timer 2 before making changes
  TimerDisable(TIMER0_BASE, TIMER_A);
      // Configure Timer 2 as a 32 bit periodic timer
  TimerConfigure ( TIMER0_BASE, TIMER_CFG_16_BIT_PAIR | TIMER_CFG_A_PERIODIC);
  TimerPrescaleSet(TIMER0_BASE, TIMER_A, TIMERPRESCALE);
      // Enable TIMER2A interrupt
  TimerIntEnable( TIMER0_BASE, TIMER_TIMA_TIMEOUT );
      // Register TIMER2A interrupt with NVIC
  IntEnable(INT_TIMER0A);

//*TIMER 2
      // Enable General Purpose Timer 2
  SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER2);
      // Disable timer 2 before making changes
  TimerDisable(TIMER2_BASE, TIMER_B);
      // Configure Timer 2 as a 32 bit periodic timer
  TimerConfigure ( TIMER2_BASE, TIMER_CFG_16_BIT_PAIR | TIMER_CFG_B_PERIODIC);
  TimerPrescaleSet(TIMER2_BASE, TIMER_B, TIMERPRESCALE);
      // Disable TIMER2A ADC Trigger
  TIMER2_CTL_R &= ~TIMER_CTL_TAOTE;
  TIMER2_CTL_R &= ~TIMER_CTL_TBOTE;
      // Enable TIMER2A interrupt
  TimerIntEnable( TIMER2_BASE, TIMER_TIMB_TIMEOUT );
      // Register TIMER2A interrupt with NVIC
  IntEnable(INT_TIMER2B);
 
//*TIMER 1
      // Enable General Purpose Timer 1
  SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER1);
      //  Disable timer 1 before making changes
  TimerDisable(TIMER1_BASE, TIMER_A);
      //  Set Timer0 as a 32bit periodic timer
  TimerConfigure(TIMER1_BASE,TIMER_CFG_16_BIT_PAIR | TIMER_CFG_A_PERIODIC);
      // Set Timer1 as a trigger for the ADC
  TIMER1_CTL_R |= TIMER_CTL_TAOTE;
  
//*SYSTICK
  SysTickEnable();
       // Enable SysTick Interrupt
  SysTickIntEnable();
  
//*ADC
      //  Initialize ADC Clock and Sample rate to 500K
  ADC_Open();
  
//*UART
  Uart_Init();
 
      // Set PendSV priority to lowest
  NVIC_SYS_PRI2_R |= 0xE0000000L;
  
  FirstThreadPt = NULL;
  CurrentThreadPt = 0;
  NextID = 1;
  NumPeriodicThreads = 0;
  SystemTime = 0;
  selectFunctionPt = 0;
  downFunctionPt   = 0;

  for( i = 0; i < NUMBER_OF_THREADS; i++ ) {
    for( j = 0; j < STACK_SIZE; j++) {
	  ThreadBlocks[i].Stack[j] = (j << 16) | 0x0000FFAA; 
	}
	ThreadBlocks[i].TopStackPt    = 0;
	ThreadBlocks[i].BottomStackPt = &(ThreadBlocks[i].Stack[STACK_SIZE - 1]);
	ThreadBlocks[i].StackPt       = &(ThreadBlocks[i].Stack[STACK_SIZE - 1]);
	ThreadBlocks[i].StackSize     = (unsigned long)STACK_SIZE;
	ThreadBlocks[i].TaskID        = 0;
	ThreadBlocks[i].NextPt        = 0;
	ThreadBlocks[i].PrevPt        = 0;
	ThreadBlocks[i].TaskPriority  = 0;
	ThreadBlocks[i].SleepCnt      = 0;
	ThreadBlocks[i].BlockPt       = 0;
  }
}

//******** OS_AddThread *************** 
// add a foregound thread to the scheduler
// Inputs: pointer to a void/void foreground task
//         number of bytes allocated for its stack
//         priority (0 is highest)
// Outputs: 1 if successful, 0 if this thread can not be added
// stack size must be divisable by 8 (aligned to double word boundary)
// stack size must be no greater than 256
// In Lab 2, you can ignore both the stackSize and priority fields
// In Lab 3, you can ignore the stackSize fields
int OS_AddThread(void(*task)(void), 
   unsigned long stackSize, unsigned long priority) {
     
  int i; // For index
  unsigned long *st; // Stack pointer 
  unsigned long ulRet;
  TCB *threadListPt, *newThreadPt, *endPt;
  
  if(stackSize > STACK_SIZE)
  	return 0; // Return fail

  ulRet = IntMasterDisable();

  if(!ulRet){
    OS_DisableIntCountStart();
  }

      //  Search for 1st available thread space
  for(i = 0; (ThreadBlocks[i].TaskID != 0) && i < 10; i++);

      // Return 0 if thread space is full
  if(i == 10)
  	return 0; // Return fail

  newThreadPt = &ThreadBlocks[i];

      // Initialize TCB Variables
  newThreadPt->StackSize    = stackSize;
  newThreadPt->TopStackPt   = &(ThreadBlocks[i].Stack[stackSize-1]);
  newThreadPt->StackPt      = &(ThreadBlocks[i].Stack[STACK_SIZE - 1]);
  newThreadPt->TaskPriority = priority;
  newThreadPt->TaskID       = NextID++;
  newThreadPt->TimesRun     = 0;
  newThreadPt->SleepCnt     = 0;
  newThreadPt->BlockPt      = 0;

  #if PRIORITY
      // Check for empty list
  if(FirstThreadPt == NULL){
    newThreadPt->NextPt = NULL; // NULL marks end
	newThreadPt->PrevPt = NULL; // NULL marks beginning
	FirstThreadPt = newThreadPt; // First thread has highest relative priority
	endPt = newThreadPt;
  }
  else{
	threadListPt = FirstThreadPt;
	while(threadListPt != NULL && threadListPt->TaskPriority <= priority) {
	  if(threadListPt->NextPt == NULL){ //Check for end
	    endPt = threadListPt; // Save last
	  }
	  threadListPt = threadListPt->NextPt;
	}
	if(threadListPt == FirstThreadPt){ // Insert at the beginning
	      // Move first thread to second place on the list
      FirstThreadPt->PrevPt = newThreadPt;
	      // Place newThread in first place on list
      newThreadPt->NextPt = FirstThreadPt;
	  newThreadPt->PrevPt = NULL; // NULL marks beginning
	  FirstThreadPt = newThreadPt;
	}
	else if (threadListPt == NULL) {  //insert at the end
	      // Place newThread at the end
	  newThreadPt->NextPt = NULL; // NULL marks the end
	  newThreadPt->PrevPt = endPt;
	      // Make second to last thread point to last
	  endPt->NextPt = newThreadPt;
	}
	else{ //Insert in the middle
		   // Set Pointers for the new thread
	  newThreadPt->NextPt = threadListPt; 
	  newThreadPt->PrevPt = threadListPt->PrevPt;
	      // Set Pointer for the thread before the new thread
	  threadListPt->PrevPt->NextPt = newThreadPt;
	      // Set Pointers for the thread after the new thread
	  threadListPt->PrevPt = newThreadPt;
	}
  }

  #else
  // ROUND ROBIN
  // Set Next and Previous Pointers
  // If no threads exist, make thread point to itself
  if(CurrentThreadPt == 0){
    newThreadPt->NextPt = &(ThreadBlocks[i]);
	newThreadPt->PrevPt = &(ThreadBlocks[i]);
	CurrentThreadPt = newThreadPt;
  }
  // Else, add thread behind the current thread
  else{
	// Save address of current thread's previous (will be refered to as previous)
	PrevPt = CurrentThreadPt->PrevPt;
	// Set previous's next to new thread
	PrevPt->NextPt = newThreadPt;
	// Set new threads's previous to previous
	newThreadPt->PrevPt = PrevPt;
	// Set current's previous to new thread
	CurrentThreadPt->PrevPt = newThreadPt;
	//Set new's next to current thread
	newThreadPt->NextPt = CurrentThreadPt;
  }
  #endif

  st = newThreadPt->StackPt;

//*Intialize Thread Stack --- copied from uOS II
      // Push PSR
  (*st) = (unsigned long)0x01000000L;
      // Push PC pointing to function code
  *(--st) = (unsigned long)task;  
      // Push LR
  *(--st) = 0xFFFFFFF9L; 
      // Push R12
  *(--st) = 0x12121212L;  
      // Push R3
  *(--st) = 0x03030303L; 
      // Push R2
  *(--st) = 0x02020202L; 
      // Push R1
  *(--st) = 0x01010101L; 
      // Push R0
  *(--st) = 0xAFAFAFAFL;  
      // Push R11
  *(--st) = 0xBBBBBBBBL; 
      // Push R10
  *(--st) = 0xAAAAAAAAL; 
      // Push R9
  *(--st) = 0x99999999L;  
      // Push R8
  *(--st) = 0x88888888L;  
      // Push R7
  *(--st) = 0x77777777L; 
      // Push R6
  *(--st) = 0x66666666L; 
      // Push R5
  *(--st) = 0x55555555L;
      // Push R4
  *(--st) = 0x44444444L; 

      // Update stack pointer
  newThreadPt->StackPt = st;

  if(!ulRet){
    OS_DisableIntCountEnd();
	IntMasterEnable();
  }
  
  return 1; // Return Success
}

// ******** OS_Kill ************
// kill the currently running thread, release its TCB memory
// input:  none
// output: none
void OS_Kill(void){
  struct os_TCB *prevTCBPt;
  struct os_TCB *nextTCBPt;
  

  // Disable Interrupts
  IntMasterDisable();

  OS_DisableIntCountStart();

  // Logically eliminate the TCB
  CurrentThreadPt->TaskID = 0;

  prevTCBPt = CurrentThreadPt->PrevPt;
  nextTCBPt = CurrentThreadPt->NextPt;
  
  // Update TCB linked list
  if(prevTCBPt == NULL){  // First thread in list
    nextTCBPt->PrevPt = NULL;
	FirstThreadPt= nextTCBPt;
  }else if(nextTCBPt == NULL){ // Last thread in list
    prevTCBPt->NextPt = NULL;
  }else{
    prevTCBPt->NextPt = nextTCBPt;
	nextTCBPt->PrevPt = prevTCBPt;
  }

  // Cause Context Switch
  OS_CPU_ContextSwitch();

  OS_DisableIntCountEnd();

  IntMasterEnable();
}

// ******** OS_Suspend ************
// suspend execution of currently running thread
// scheduler will choose another thread to execute
// Can be used to implement cooperative multitasking 
// Same function as OS_Sleep(0)
// input:  none
// output: none
void OS_Suspend(void){
  OS_CPU_ContextSwitch();
}


// *********************************************
// One public function will reset the 32-bit counter to 0.  
//
void OS_ClearMsTime(void){
  SystemTime = 0;
}

//
// Will create a periodc thread using timer 2
//! \param task is a function pointer
//! \param freq is frequency in hertz
//! \param priority is the interrupt priority
// 

//******** OS_AddButtonTask *************** 
// add a background task to run whenever the Select button is pushed
// Inputs: pointer to a void/void background function
//         priority 0 is highest, 5 is lowest
// Outputs: 1 if successful, 0 if this thread can not be added
// The first time the function is called, the task is tied to the Select button
// The seconde time this function is called, the task is tied to the Down button
// It is assumed that the user task will run to completion and return
// This task can not spin, block, loop, sleep, or kill
// This task can call OS_Signal  OS_bSignal	 OS_AddThread
// This task does not have a Thread ID
// In labs 2 and 3, this command will be called 0 or 1 times
// In lab 2, the priority field can be ignored
// In lab 3, there will be up to four background threads, and this priority field 
//           determines the relative priority of these four threads
int OS_AddButtonTask(void(*task)(void), unsigned long priority){
   unsigned long ulRet;
   
  //
  // ***NOTE*** THIS INITIALIZATION CODE WAS COPIED FROM gpio_jtag.c
  // Configure the push button as an input and enable the pin to interrupt on
  // the falling edge (i.e. when the push button is pressed).
  //
  if(selectFunctionPt ==0){
   	ulRet = IntMasterDisable();
   
   if(!ulRet){
     OS_DisableIntCountStart();
   }

	SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
    GPIOPinTypeGPIOInput(GPIO_PORTF_BASE, GPIO_PIN_1);
    GPIOPadConfigSet(GPIO_PORTF_BASE, GPIO_PIN_1, GPIO_STRENGTH_2MA,
                     GPIO_PIN_TYPE_STD_WPU);
    GPIOIntTypeSet(GPIO_PORTF_BASE, GPIO_PIN_1, GPIO_FALLING_EDGE);
    IntPrioritySet(INT_GPIOF, priority);
    GPIOPinIntEnable(GPIO_PORTF_BASE, GPIO_PIN_1);
    IntEnable(INT_GPIOF);

    selectFunctionPt = task;
   
   if(!ulRet){
     OS_DisableIntCountEnd();
	 IntMasterEnable();
   }

    return 1;
  }
  else if(downFunctionPt == 0){
    ulRet = IntMasterDisable();
   
   if(!ulRet){
     OS_DisableIntCountStart();
   }	
	//
    // ***NOTE*** THIS INITIALIZATION CODE WAS COPIED FROM gpio_jtag.c
    // Configure the push button as an input and enable the pin to interrupt on
    // the falling edge (i.e. when the push button is pressed).
    //
    SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOE);
	GPIOPinTypeGPIOInput(GPIO_PORTE_BASE, GPIO_PIN_1);
    GPIOPadConfigSet(GPIO_PORTE_BASE, GPIO_PIN_1, GPIO_STRENGTH_2MA,
                     GPIO_PIN_TYPE_STD_WPU);
    GPIOIntTypeSet(GPIO_PORTE_BASE, GPIO_PIN_1, GPIO_FALLING_EDGE);
    IntPrioritySet(INT_GPIOE, priority);
    GPIOPinIntEnable(GPIO_PORTE_BASE, GPIO_PIN_1);
    IntEnable(INT_GPIOE);

    downFunctionPt = task;

   if(!ulRet){
     OS_DisableIntCountEnd();
	 IntMasterEnable();
   }

	return 1;
  }
  else{
    return 0;
  }
}

//******** OS_AddPeriodicThread *************** 
// add a background periodic task
// typically this function receives the highest priority
// Inputs: pointer to a void/void background function
//         period given in system time units
//         priority 0 is highest, 5 is lowest
// Outputs: 1 if successful, 0 if this thread can not be added
// It is assumed that the user task will run to completion and return
// This task can not spin, block, loop, sleep, or kill
// This task can call OS_Signal  OS_bSignal	 OS_AddThread
// You are free to select the time resolution for this function
// This task does not have a Thread ID
// In lab 2, this command will be called 0 or 1 times
// In lab 2, the priority field can be ignored
// In lab 3, this command will be called 0 1 or 2 times
// In lab 3, there will be up to four background threads, and this priority field 
//           determines the relative priority of these four threads
int OS_AddPeriodicThread( void( *task )( void ), unsigned long period, unsigned long priority ) {
  unsigned long ulRet;

  if(NumPeriodicThreads == 0){
    ulRet = IntMasterDisable();

	if(!ulRet){
	  OS_DisableIntCountStart();
	}
	
	// Associate thread with interrupt
    functionPt = task;

    // Set interrupt priority
    IntPrioritySet(INT_TIMER0A, priority);

    // Disable Timer 0 countdown
    TimerDisable( TIMER0_BASE, TIMER_A );

    // Configure frequency for Thread
    TimerLoadSet( TIMER0_BASE, TIMER_A, period/TIMERPRESCALE );
    Period1 = period;

    // Enable Timer 0 countdown
    TimerEnable( TIMER0_BASE, TIMER_A );

	NumPeriodicThreads++;

	if(!ulRet){
	  OS_DisableIntCountEnd();
	  IntMasterEnable();
	}

    return 1;

  }else if(NumPeriodicThreads == 1){
    ulRet = IntMasterDisable();
	
	if(!ulRet){
	  OS_DisableIntCountStart();
	}

	// Associate thread with interrupt
    function2Pt = task;

    // Set interrupt priority
    IntPrioritySet(INT_TIMER2B, priority);

    // Disable Timer 2 countdown
    TimerDisable( TIMER2_BASE, TIMER_B );

    // Configure frequency for Thread
    TimerLoadSet( TIMER2_BASE, TIMER_B, period/TIMERPRESCALE );
	Period2 = period;

    // Enable Timer 2 countdown
    TimerEnable( TIMER2_BASE, TIMER_B );

	NumPeriodicThreads++;

    if(!ulRet){
	  OS_DisableIntCountEnd();
	  IntMasterEnable();
	}

    return 1;

  }else{
    return 0;
  }
}


//******** OS_Launch *************** 
// start the scheduler, enable interrupts
// Inputs: number of 20ns clock cycles for each time slice
//         you may select the units of this parameter
// Outputs: none (does not return)
// In Lab 2, you can ignore the theTimeSlice field
// In Lab 3, you should implement the user-defined TimeSlice field
void OS_Launch(unsigned long theTimeSlice){
  //
  //	Enable system interrupts
  //
  IntMasterEnable();

  // Set time slice
  SysTickPeriodSet(theTimeSlice);
  TimeSlice = theTimeSlice;

  #if PRIORITY
  // Set Current Thread for first context switch
  CurrentThreadPt = FirstThreadPt;
  #endif

  // For running testmain1()
  OS_CPU_RunFirstContext();

  while(1){}
}

// ******** OS_InitSemaphore ************
// initialize semaphore 
// input:  pointer to a semaphore
// output: none
void OS_InitSemaphore(Sema4Type *semaPt, long value) {
  semaPt->Value = value;
  //semaPt->BlockListPt = NULL;
}
// ******** OS_Signal ************
// increment semaphore, wakeup blocked thread if appropriate 
// input:  pointer to a counting semaphore
// output: none

void OS_Signal(Sema4Type *semaPt) {
  unsigned long ulRet;
  TCB *tempPt, *temp2Pt;

  // Disable system interrupts
  ulRet = IntMasterDisable();
  
  if(!ulRet){
    OS_DisableIntCountStart();
  }
  #if PRIORITY
  // If a blocked thread exists
  if(semaPt->Value < 0){
    tempPt = FirstThreadPt;

	while(tempPt->BlockPt!=semaPt && tempPt != NULL){ // Find first thread blocked on this sema4
	  tempPt=tempPt->NextPt;
	}

	temp2Pt = tempPt->NextPt; // Point to next thread to see if it is a better candidate to run
	
    while(temp2Pt != NULL){ // Stop when next thread is a higher priority or end of list is reached
      if(tempPt->TaskPriority == temp2Pt->TaskPriority){ 
	        // Check if thread is blocked and if it has run less than current thread being pointed to by tempPt
	    if(tempPt->TimesRun > temp2Pt->TimesRun && temp2Pt->SleepCnt == 0 && temp2Pt-> BlockPt == semaPt){
	     tempPt = temp2Pt; // tempPt points to blocked thread with lowest run time
	    }
	    temp2Pt = temp2Pt->NextPt;
	  }
	  else{
	    temp2Pt = NULL;
	  }
    }
    tempPt->BlockPt=NULL; // Unblock the thread
	
	// Switch to newly unblocked thread if priority is greater than 
	if(tempPt->TaskPriority < CurrentThreadPt->TaskPriority){
	  OS_CPU_ContextSwitch();
	}
  } // end if
  
  #else
  index = 0;
  temp2Pt = NULL;
  while(index < NUMBER_OF_THREADS){
	tempPt = &ThreadBlocks[index];
	if(tempPt->BlockPt == semaPt){
	   if(temp2Pt == NULL){
	     temp2Pt = tempPt;
	   }
	   else if(tempPt->TimesRun < temp2Pt->TimesRun){
	     temp2Pt = tempPt;
       }
	}
	index++;		
  }
  temp2Pt->BlockPt = NULL;
  #endif	

  //if(semaPt->Value > 1)
    //semaPt->Value = semaPt->Value;

  // Increment semaphore value
  semaPt -> Value += 1;

  // Enable system interrupts
  if(!ulRet){
    OS_DisableIntCountEnd();
	IntMasterEnable();
  }
} 

// ******** OS_Wait ************
// if the semaphore is 0 then spin/block
// if the semaphore is 1, then clear semaphore to 0
// input:  pointer to a binary semaphore
// output: none
void OS_Wait(Sema4Type *semaPt) {
  unsigned long ulRet;
  
  ulRet = IntMasterDisable();
  
  if(!ulRet){
    OS_DisableIntCountStart();
  }
  	
  semaPt->Value--;

  // Block if semaphore is out of 
  if(semaPt->Value < 0){
    CurrentThreadPt->BlockPt = semaPt;
	OS_CPU_ContextSwitch();
  }

  // Enable system interrupts if enabled before function call
  if(!ulRet){
    OS_DisableIntCountEnd();
    IntMasterEnable();
  }
}

// ******** OS_Sleep ************
// place this thread into a dormant state
// input:  number of msec to sleep
// output: none
// You are free to select the time resolution for this function
// OS_Sleep(0) implements cooperative multitasking
void OS_Sleep(unsigned long sleepTime) {
  //
  //  Store sleep time in current threads control block
  //
  CurrentThreadPt->SleepCnt = sleepTime;
  //
  //  While SleepCnt of current thread is greater than 0
  //
  while ( CurrentThreadPt->SleepCnt > 0 ) {
    OS_CPU_ContextSwitch();
  }
}

// ******** OS_Time ************
// reads a timer value 
// Inputs:  none
// Outputs: time in 20ns units, 0 to max
// The time resolution should be at least 1us, and the precision at least 12 bits
// It is ok to change the resolution and precision of this function as long as 
//   this function and OS_TimeDifference have the same resolution and precision 
// what I recommend for this implementation is simply return NVIC_ST_CURRENT_Rs
unsigned long OS_Time(void) {
  return NVIC_ST_CURRENT_R;
  //return SysTickPeriodGet();
}

// ******** OS_TimeDifference ************
// Calculates difference between two times
// Inputs:  two times measured with OS_Time
// Outputs: time difference in 20ns units 
// The time resolution should be at least 1us, and the precision at least 12 bits
// It is ok to change the resolution and precision of this function as long as 
//   this function and OS_Time have the same resolution and precision 
unsigned long OS_TimeDifference(unsigned long start, unsigned long stop) {
  //unsigned long tempTime;
  if(stop > start){
  	return (stop - start);
  }
  else{
    //tempTime = NVIC_ST_RELOAD_R - start;
	//return stop + tempTime;
	return stop + (NVIC_ST_RELOAD_R - start);
  }
}

// ******** OS_MsTime ************
// reads the current time in msec (from Lab 1)
// Inputs:  none
// Outputs: time in ms units
// You are free to select the time resolution for this function
unsigned long OS_MsTime(void) {
  return SystemTime;
}

// ******** OS_bWait ************
// if the semaphore is 0 then spin/block
// if the semaphore is 1, then clear semaphore to 0
// input:  pointer to a binary semaphore
// output: none
void OS_bWait(Sema4Type *semaPt) {
  unsigned long ulRet;
  
  ulRet = IntMasterDisable();

  // Block if semaphore is out of 
  if(semaPt->Value == 0){
    CurrentThreadPt->BlockPt = semaPt;
	OS_CPU_ContextSwitch();
  }
  semaPt->Value=0; // Otherwise use resource
  
  // Enable system interrupts if enabled before function call
  if(!ulRet){
    IntMasterEnable();
  }
}
// ******** OS_bSignal ************
// set semaphore to 1, wakeup blocked thread if appropriate 
// input:  pointer to a binary semaphore
// output: none
void OS_bSignal(Sema4Type *semaPt) {
  unsigned long ulRet;
  TCB *tempPt=FirstThreadPt, *temp2Pt;
  
  ulRet = IntMasterDisable();
  
 	while(tempPt->BlockPt!=semaPt && tempPt != NULL){ // Find first thread blocked on this sema4
	  tempPt=tempPt->NextPt;
	}
	temp2Pt = tempPt->NextPt; // Point to next thread to see if it is a better candidate to run
	
    while(temp2Pt != NULL){ // Stop when next thread is a higher priority or end of list is reached
      if(tempPt->TaskPriority == temp2Pt->TaskPriority){ 
	        // Check if thread is blocked and if it has run less than current thread being pointed to by tempPt
	    if((tempPt->TimesRun) > (temp2Pt->TimesRun ) && temp2Pt->SleepCnt == 0 && temp2Pt-> BlockPt == semaPt){
	     tempPt = temp2Pt; // tempPt points to blocked thread with lowest run time
	    }
	    temp2Pt = temp2Pt->NextPt;
	  }
	  else{
	    temp2Pt = NULL;
	  }
    }
    tempPt->BlockPt=NULL; // Unblock the thread
	semaPt->Value=1;
	
	    // Switch to newly unblocked thread if priority is greater than 
	if(tempPt->TaskPriority < CurrentThreadPt->TaskPriority){
	  OS_CPU_ContextSwitch();
	}

  if(!ulRet){
    IntMasterEnable();
  }
}

// ******** OS_Fifo_Init ************
// Initialize the Fifo to be empty
// Inputs: size
// Outputs: none 
// In Lab 2, you can ignore the size field
// In Lab 3, you should implement the user-defined fifo size
// In Lab 3, you can put whatever restrictions you want on size
//    e.g., 4 to 64 elements
//    e.g., must be a power of 2,4,8,16,32,64,128
void OS_Fifo_Init(unsigned long size) {
  Fifo_Put_Pt = Fifo_Get_Pt = 0;
}

// ******** OS_Fifo_Put ************
// Enter one data sample into the Fifo
// Called from the background, so no waiting 
// Inputs:  data
// Outputs: true if data is properly saved,
//          false if data not saved, because it was full
// Since this is called by interrupt handlers 
//  this function can not disable or enable interrupts
int OS_Fifo_Put(unsigned long data) {
  if(( Fifo_Put_Pt - Fifo_Get_Pt ) & ~(FIFO_SIZE-1)){
    return(FAIL);
  }
  Fifo[Fifo_Put_Pt & (FIFO_SIZE-1)] = data;
  Fifo_Put_Pt++;
  return(SUCCESS);
} 

// ******** OS_Fifo_Get ************
// Remove one data sample from the Fifo
// Called in foreground, will spin/block if empty
// Inputs:  none
// Outputs: data
unsigned long spinCnt=0; 
unsigned long OS_Fifo_Get(void){
  unsigned long dataGet;
  //
  //  Spin while fifo is empty
  //
  while( Fifo_Put_Pt == Fifo_Get_Pt ){spinCnt++;}
  //
  //  Retrieve Data
  //
  dataGet = Fifo[ Fifo_Get_Pt & (FIFO_SIZE-1)];
  Fifo_Get_Pt++;
  return(dataGet);
}

// ******** OS_Fifo_Size ************
// Check the status of the Fifo
// Inputs: none
// Outputs: returns the number of elements in the Fifo
//          greater than zero if a call to OS_Fifo_Get will return right away
//          zero or less than zero if the Fifo is empty 
//          zero or less than zero  if a call to OS_Fifo_Get will spin or block
long OS_Fifo_Size(void){
  return Fifo_Put_Pt - Fifo_Get_Pt;
}


// ******** OS_MailBox_Init ************
// Initialize communication channel
// Inputs:  none
// Outputs: none
void OS_MailBox_Init(void){
  //
  //  Mailbox Initially empty
  //
  Sema4_MailBox_Valid.Value = 0;
  //
  //  Mailbox is ready to be written to
  //
  Sema4_MailBox_Free.Value = 1;
}

// ******** OS_MailBox_Send ************
// enter mail into the MailBox
// Inputs:  data to be sent
// Outputs: none
// This function will be called from a foreground thread
// It will spin/block if the MailBox contains data not yet received 
void OS_MailBox_Send(unsigned long data) {
  //
  //  Spin/Block if mailbox has not been read 
  //  since the last write
  //
  OS_Wait(&Sema4_MailBox_Free);

  //
  //  Place data into the mailbox
  //
  MailBox = data;

  //
  // Signal that data is available
  // 
  OS_Signal(&Sema4_MailBox_Valid);

}

// ******** OS_MailBox_Recv ************
// remove mail from the MailBox
// Inputs:  none
// Outputs: data received
// This function will be called from a foreground thread
// It will spin/block if the MailBox is empty 
long mailData;
unsigned long OS_MailBox_Recv(void){
  //
  //  Spin/Block if mailbox is empty
  //
  OS_Wait(&Sema4_MailBox_Valid);
  
  //
  // Retrieve Mail
  //
  mailData = MailBox;

  //
  // Signal Mailbox is free
  //
  OS_Signal(&Sema4_MailBox_Free);

  //
  //  Return Data
  //
  return mailData;
}
//******** OS_Id *************** 
// returns the thread ID for the currently running thread
// Inputs: none
// Outputs: Thread ID, number greater than zero 
unsigned long OS_Id(void){
  return CurrentThreadPt -> TaskID;
}

// ************* IntDebugger **********
void intDebugger (void){
	unsigned long ulStatus;
	//
	// Get interrupt staus bit
	//
	ulStatus = TimerIntStatus( TIMER1_BASE, true );
	//
	// Acknowledge interrupt
	//
	TimerIntClear( TIMER1_BASE, ulStatus );
	//
	//  Toggle port PC7
	//
	GPIO_PORTC_DATA_R ^= 0x80; 
}

void OS_FindNextThread(void){
  #if PRIORITY
  TCB *tempPt  = FirstThreadPt;
  TCB *temp2Pt = NULL;
  
 
  // Find first non-sleeping, non-blocked Thread
  while(temp2Pt == NULL){
    if(tempPt->SleepCnt == 0 && tempPt-> BlockPt == NULL) {
	  temp2Pt=tempPt;
    }
	else{
	  tempPt = tempPt->NextPt;
	}
  }
  
  //GPIO_PORTB_DATA_R &= ~(0x01 << ((CurrentThreadPt->TaskID)-1));

  temp2Pt = temp2Pt->NextPt;

  while(temp2Pt != NULL){ // Stop when next thread is a higher priority or end of list is reached
    if(tempPt->TaskPriority == temp2Pt->TaskPriority){ 
	   // Check if thread is blocked and if it has run less than current thread being pointed to by tempPt
      if(tempPt->TimesRun > temp2Pt->TimesRun && temp2Pt->SleepCnt == 0 && temp2Pt-> BlockPt == NULL){
        tempPt = temp2Pt; // tempPt points to blocked thread with lowest run time
	  }
      temp2Pt = temp2Pt->NextPt;
	}
	else{ // temp2Pt is now a lower priority then tempPt
	  temp2Pt = NULL;
	}
  }

  tempPt->TimesRun++;
  CurrentThreadPt = tempPt;
  
  #else
  // Round Robin
  CurrentThreadPt->TimesRun++;
  GPIO_PORTB_DATA_R &= ~(0x01 << ((CurrentThreadPt->TaskID)-1));
  CurrentThreadPt = CurrentThreadPt->NextPt;
  while(CurrentThreadPt->SleepCnt > 0 || CurrentThreadPt->BlockPt != NULL){
    CurrentThreadPt = CurrentThreadPt->NextPt;
  }
  #endif
  
  TaskRecord(CurrentThreadPt->TaskID,SystemTime);

  
  //GPIO_PORTB_DATA_R |= (0x01 << ((CurrentThreadPt->TaskID)-1));
  
  /*if(CurrentThreadPt->TaskID == 2)
    GPIO_PORTB_DATA_R ^= 0x08;*/

  // Reset Systick to give full timeslice
  NVIC_ST_CURRENT_R = 0;
  
  
}

char tempBuf[50];
int numChar;
void OS_PrintJitter(void){
  numChar = sprintf(tempBuf, "MinJitter1: %d\n\rMaxJitter1: %d\n\r", MinJitter1, MaxJitter1);
  Uart_SendString(tempBuf);
  numChar = sprintf(tempBuf, "MinJitter2: %d\n\rMaxJitter2: %d\n\r", MinJitter2, MaxJitter2);
  Uart_SendString(tempBuf);
}

void OS_ClearTaskRecord(void){
  unsigned long i;
  for(i=0;i<RECORD_SIZE;i++){
    TaskRecordID[i]=0xFFFF0000;
    TaskRecordT[i]=0;
  }
  RecordIndex=0;
}

void OS_PrintTaskRecord(void){
  unsigned long i,time,id;
  char buffer[100];
  for(i=0;i<RECORD_SIZE;i++){
	time = TaskRecordT[i];
	id = TaskRecordID[i];
	if (id == 25000){
	  sprintf(buffer,"\n\rPeriodic Thread      1     -|-   Time = %5d ms", 2*time);
	}
	else if (id == 50000){
	  sprintf(buffer,"\n\rPeriodic Thread      2     -|-   Time = %5d ms", 2*time);
	}
	else{
      sprintf(buffer,"\n\rThread ID =        %3d     -|-   Time = %5d ms",id, 2*time);
	}
    Uart_SendString(buffer);
	OS_Sleep(8);
  }
}

static long StartTime;
static long AverageTime = 0;
static long MaxDisableTime = 0;

void OS_DisableIntCountStart(void){
  StartTime = OS_Time();
}

void OS_DisableIntCountEnd(void){
  long TotalTime;
  TotalTime = OS_TimeDifference(OS_Time(), StartTime);

  if(TotalTime > MaxDisableTime){
    MaxDisableTime = TotalTime;
  }

  if(!AverageTime)
    AverageTime = TotalTime ;
  else
    AverageTime = (AverageTime + TotalTime)/2 ;
}



